#ifndef _SERVO_MANAGER_H_
#define _SERVO_MANAGER_H_
#include "ServoUbt.h"
#include "ServoSg90.h"


#define SERVO_MAX_CNT					32

// Offsets in EEPROM
// 0~64 bytes
#define EEPROM_SERVO_OFFSET		0

typedef enum _ServoType
{
	SERVO_UBT = 0,
	SERVO_SG90
} ServoType_t;


/**
* @brief:
* Servo managers for controlling all servos.
*
* If servo not support offset self, using EEPROM to support! And must
* be applied in the high-level application.
*
* idx start from 0: [0, servoCnt)
*/
class CServoManager
{
public:
	CServoManager();
	~CServoManager();

	ServoRet_t Init(ServoType_t type, int servoCnt, int *idsOrPins, uint16_t *initAngles,
		int16_t offsetMin, int16_t offsetMax, HardwareSerial *serl = NULL, int waitMsec = 10);
	void Uninit();
	int GetServoCount();
	void SetOffsetLocalSupport(bool support);
	void SetToInitAngles();
	uint16_t GetInitAngle(int idx);

	ServoRet_t SetAngle(int idx, uint16_t angle, uint32_t msec);
	ServoRet_t GetAngle(int idx, uint16_t &angle);
	ServoRet_t SetOffset(int idx, int16_t offset);
	ServoRet_t GetOffset(int idx, int16_t &offset);
	ServoRet_t SetId(int idx, uint8_t newid);
	ServoRet_t SetLed(int idx, bool on);
	ServoRet_t Terminate(int idx);
	ServoRet_t GetFirmware(int idx, uint32_t &firmware);
	ServoRet_t Lock(int idx);
	ServoRet_t Unlock(int idx);

protected:
	// EEPROM
	void resetOffsets();
	void loadOffsets();
	void saveOffsets();
	ServoRet_t getOffset(int idx, int16_t &offset);
	ServoRet_t setOffset(int idx, int16_t offset);

protected:
	int m_servoCnt;
	CServoBase *m_servos[SERVO_MAX_CNT];
	uint16_t m_initAngles[SERVO_MAX_CNT];

	bool m_offsetLocalSupport;
	int16_t m_offsetMin;
	int16_t m_offsetMax;
	int16_t m_offsets[SERVO_MAX_CNT];
};


#endif


